
//强大的kalman滤波算法
//Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
//R:测量噪声，R增大，动态响应变慢，收敛稳定性变好


#include "Kalman.h"
// #include "App_Main.h"
#include <stdint.h>


KFP_S KFP_ADC_RTD =    {100, 0, 0, 0, 40, 450};
KFP_S KFP_ADC_Probe1 = {100, 0, 0, 0, 40, 400};
KFP_S KFP_ADC_Probe2 = {100, 0, 0, 0, 40, 400};


/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float kalmanFilter(KFP_S *kfp,float input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    
    //卡尔曼增益方程：卡尔曼增益 = k1-1时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = kfp->Now_P  / (kfp->Now_P + kfp->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input -kfp->out); //因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
    
    return kfp->out;
}

//中位值平均滤波法
#define N 20
float MedianFilter(float xn)
{
    static int index = 0;
    static float buffer[N];
    int i = 0;
    int j = 0;
    float temp = 0;

    buffer[index] = xn;
    index++;
    if(index >= N) index = 0;

    for (j = 0; j < (N - 1); j++)
    {
        for (i = 0; i < ((N - 1) - j); i++)
        {
            if (buffer[i] > buffer[i + 1])
            {
                temp = buffer[i];
                buffer[i] = buffer[i + 1];
                buffer[i + 1] = temp;
            }
        }
    }

    return ((buffer[(N-1)/2 + 1] + buffer[(N-1)/2] + buffer[(N-1)/2 - 1]) / 3);
}
float MedianFilterA(float xn)
{
    static int index = 0;
    static float buffer[N];
    int i = 0;
    int j = 0;
    float temp = 0;

    buffer[index] = xn;
    index++;
    if(index >= N) index = 0;

    for (j = 0; j < (N - 1); j++)
    {
        for (i = 0; i < ((N - 1) - j); i++)
        {
            if (buffer[i] > buffer[i + 1])
            {
                temp = buffer[i];
                buffer[i] = buffer[i + 1];
                buffer[i + 1] = temp;
            }
        }
    }

    return ((buffer[(N-1)/2 + 1] + buffer[(N-1)/2] + buffer[(N-1)/2 - 1]) / 3);
}
float MedianFilterB(float xn)
{
    static int index = 0;
    static float buffer[N];
    int i = 0;
    int j = 0;
    float temp = 0;

    buffer[index] = xn;
    index++;
    if(index >= N) index = 0;

    for (j = 0; j < (N - 1); j++)
    {
        for (i = 0; i < ((N - 1) - j); i++)
        {
            if (buffer[i] > buffer[i + 1])
            {
                temp = buffer[i];
                buffer[i] = buffer[i + 1];
                buffer[i + 1] = temp;
            }
        }
    }

    return ((buffer[(N-1)/2 + 1] + buffer[(N-1)/2] + buffer[(N-1)/2 - 1]) / 3);
}
